/**
 * @file controller_node.cpp
 * @author czj
 * @brief
 * @version 0.1
 * @date 2023-06-07
 *
 * @copyright Copyright (c) 2023
 *
 */
#include "controller_node.h"
#include "control_utils.h"

using namespace std;

namespace vtd_pnc
{
    ControllerNode::ControllerNode()
    {
        ros::NodeHandle n("~"); // 句柄

        reach_goal = false;

        // setup subscriber
        cur_location_sub = n.subscribe("/cicv_location", 10, &ControllerNode::callbackLocation, this);
        ref_trajectory_sub = n.subscribe("/cicv_amr_trajectory", 10, &ControllerNode::callbackRefTraj, this);

        // setup publishers
        control_cmd_pub = n.advertise<common_msgs::Control_Test>("/control_test", 10);
        traj_rviz_pub = n.advertise<nav_msgs::Path>("/traj", 10);                               // 可视化轨迹
        cur_pose_rviz_pub = n.advertise<geometry_msgs::PoseStamped>("/cur_pose", 10);           // 可视化车辆位姿
        preview_point_rviz_pub = n.advertise<visualization_msgs::Marker>("/preview_point", 10); // 可视化预瞄点
        predict_point_rviz_pub = n.advertise<visualization_msgs::Marker>("/predict_point", 10); // 可视化预测点
    }

    /**
     * @brief 主循环
     *
     */
    void ControllerNode::MainLoop() {
        double frequency = 100.0;
        ros::Rate rate(frequency);

        ros::NodeHandle n("~"); // 句柄
        // 初始化控制器参数
        initLonCon(n);
        initLatCon(n);
        initVehicleParameters(n);
        PidConf distance_pid_conf(distance_kp, distance_ki, distance_kd, 0);
        Pid distance_pid = Pid(distance_pid_conf);
        PidConf veolicty_pid_conf(velocity_kp, velocity_ki, velocity_kd, 0, 3.0);
        Pid veolicty_pid = Pid(veolicty_pid_conf);
        LQR lateral_lqr = LQR(Q, R, eps, max_iterations, vehicle_config);
        lateral_lqr.eps = eps;
        lateral_lqr.maxIteration_ = max_iterations;

        const double G = 9.8;
        double cur_dist = 0.0;
        double velComp = 0.0;
        double accComp = 0.0;
        double target_acc = 0.0;
        const double constant_speed = 3.6;
        const double park_speed = 2.0;

        // 打印Log
        // 获取当前系统时间
        auto current_time = std::chrono::system_clock::now();
        std::time_t time_t_date = std::chrono::system_clock::to_time_t(current_time);

        // 将时间格式化为字符串
        std::tm tm_date = *std::localtime(&time_t_date);
        std::stringstream ss;
        ss << std::put_time(&tm_date, "/home/yq/log/pid_log%Y-%m-%d_%H-%M-%S.csv");
        std::string filename = ss.str();
        std::ofstream  outFile = std::ofstream(filename, std::ios_base::app);
        outFile << "dist error" << "," << "velComp" << ","<< "preview_point.velocity + velComp "<< ","<<
        "velocity error" << "," << "accComp" << "," << "target_acc" << std::endl;

        // 创建误差数据写入流
        std::ofstream outFile_error("/home/yq/log/Error.csv");
        outFile_error << "velocity error" << "," << "acceleration error" << ","
        << "lateral error" << "," << "yaw error" << "," << "duration" << std::endl;

        while (ros::ok() && !reach_goal) {
            // 获取开始时间
            auto start = std::chrono::high_resolution_clock::now();

            if (ref_trajectory_ptr == nullptr)
            {
//                ROS_INFO("ref_trajectory_ptr is nullptr.");
                // init_point_ref_trajectory_ptr.position.x = cur_pose.x;
                // init_point_ref_trajectory_ptr.position.y = cur_pose.y;
                ros::spinOnce();
                continue;
            }

            // 获取预测点、预瞄点
            perception_msgs::TrajectoryPoint preview_point;
            preview_point = FindPreviewPoint(preview_time, cur_pose, ref_trajectory_ptr, true);


            // 纵向控制
            cur_dist = distanceBetweenPoints(cur_pose.x, cur_pose.y, preview_point.position.x, preview_point.position.y);
            distance_pid.SetRealRatio(cur_dist);
            distance_pid.SetTarget(0);
            velComp = -(distance_pid.Output());

            veolicty_pid.SetRealRatio(cur_pose.v);
//            if (ref_trajectory_ptr->trajectoryinfo.total_path_length < 10.0) {
//                veolicty_pid.SetTarget(park_speed); // preview_point.velocity + velComp
//            } else {
//                veolicty_pid.SetTarget(constant_speed); // preview_point.velocity + velComp
//            }

            veolicty_pid.SetTarget(preview_point.velocity + velComp);
            accComp = veolicty_pid.Output();
            target_acc = accComp + preview_point.a + G * sin(degreesToRadians(cur_pose.pitch)); // 加速度补偿 + 加速度 + 坡道补偿

            ROS_INFO("-------------------PID------------------");
            ROS_INFO("cur_dist: %f", cur_dist);
            ROS_INFO("velComp: %f", velComp);
            ROS_INFO("preview_point.velocity + velComp: %f", preview_point.velocity + velComp);
            ROS_INFO("velocity error: %f", preview_point.velocity + velComp - cur_pose.v);
            ROS_INFO("accComp: %f", accComp);
            ROS_INFO("target_acc: %f", target_acc);
            ROS_INFO("-----------------------------------------");

            outFile << std::fixed << std::setprecision(6) << cur_dist << "," << velComp << ","<< preview_point.velocity + velComp << ","
            << preview_point.velocity + velComp - cur_pose.v << ","<< accComp << "," << target_acc << std::endl;

            ExecutiveControl(target_acc, control_cmd);

            // 横向控制
            lateral_lqr.LatExecutiveControl(preview_point, cur_pose, control_cmd);

            //TODO：（期望加速度+加速度补偿，当前速度，当前档位）油门刹车标定表查表

            if ( cur_dist < 0.1 ) { // && cur_pose.v < 0.1
                reach_goal = true;
                // 初始化Control_Test消息
                common_msgs::Control_Test msg;
                msg.header.stamp = ros::Time::now();
                msg.header.frame_id = "base_link";
                msg.Gear = 0;              // 假设1代表前进档
                msg.ThrottlePedal = 0; // 油门踏板位置范围0~100，0表示未踩油门
                msg.BrakePedal = 100;       // 制动踏板位置，0表示未踩刹车
                msg.SteeringAngle = 0;    // 方向盘转角，假设10.0度向右

                ROS_INFO("Park now ThrottlePedal:%f, BrakePedal:%f, SteeringAngle:%f",
                         msg.ThrottlePedal, msg.BrakePedal, msg.SteeringAngle);
                // 发布消息
                control_cmd_pub.publish(msg);
                break;
            }

            if (ref_trajectory_ptr->trajectoryinfo.total_path_length < 10.0 && fabs(control_cmd.steer) > 5.0) {
                control_cmd.steer = 0.0;    // 方向盘转角，假设10.0度向右
            }

            // 初始化Control_Test消息
            common_msgs::Control_Test msg;
            msg.header.stamp = ros::Time::now();
            msg.header.frame_id = "base_link";
            msg.Gear = control_cmd.gear;              // 假设1代表前进档
            msg.ThrottlePedal = control_cmd.throttle; // 油门踏板位置范围0~100，0表示未踩油门
            msg.BrakePedal = control_cmd.brake;       // 制动踏板位置，0表示未踩刹车
            msg.SteeringAngle = control_cmd.steer;    // 方向盘转角，假设10.0度向右

            ROS_INFO("ThrottlePedal:%f, BrakePedal:%f, SteeringAngle:%f",
                     control_cmd.throttle, control_cmd.brake, control_cmd.steer);
            // 发布消息
            control_cmd_pub.publish(msg);

            // 可视化traj、preview_point、predict_point、cur_pose
            publishTrajectory(traj_rviz_pub, ref_trajectory_ptr, init_point_ref_trajectory_ptr);
            publishPreviewPoint(preview_point_rviz_pub, preview_point, init_point_ref_trajectory_ptr);
//            publishPredictPoint(predict_point_rviz_pub, predict_point, init_point_ref_trajectory_ptr);
            publishCurPose(cur_pose_rviz_pub, cur_pose, init_point_ref_trajectory_ptr);

            // 获取结束时间
            auto end = std::chrono::high_resolution_clock::now();

            // 计算持续时间
            auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);

            // 打印耗时
            std::cout << "Duration: " << duration.count() << " milliseconds" << std::endl;

            outErrorToFile(outFile_error, ref_trajectory_ptr, cur_pose, duration.count());

            ros::spinOnce();
            rate.sleep();
        }
    }

    void ControllerNode::callbackLocation(const perception_msgs::PerceptionLocalization::ConstPtr &msg) {
        // 从消息中提取位置信息
        cur_pose.x = msg->position_x;
        cur_pose.y = msg->position_y;
        cur_pose.z = msg->position_z;

        // 从消息中提取航向角信息
        cur_pose.yaw = msg->yaw;
        cur_pose.pitch = msg->pitch;

        // 从消息中提取速度信息
        cur_pose.vx = msg->velocity_x;
        cur_pose.vy = msg->velocity_y;

        // 计算合速度，假设速度在平面上，忽略Z轴速度分量
        cur_pose.v = sqrt(cur_pose.vx * cur_pose.vx + cur_pose.vy * cur_pose.vy);
        cur_pose.acc = sqrt(msg->accel_x * msg->accel_x + msg->accel_x * msg->accel_x);

        // 曲率暂不使用
        cur_pose.cur = 0;

        init_point_ref_trajectory_ptr.position.x = cur_pose.x;
        init_point_ref_trajectory_ptr.position.y = cur_pose.y;

        //        ROS_INFO("X: %f, Y: %f, Yaw: %f V: %f", cur_pose.x, cur_pose.y, cur_pose.z, cur_pose.yaw, cur_pose.v);
    }

    void ControllerNode::callbackRefTraj(const perception_msgs::Trajectory::ConstPtr &msg) {
        perception_msgs::Trajectory ref_trajectory = *msg;

        ref_trajectory_ptr = std::make_shared<perception_msgs::Trajectory>(ref_trajectory);
        calcTrajectoryCurv(ref_trajectory_ptr);
//         ROS_INFO("Trajectory Point Num: %d", ref_trajectory_ptr->trajectoryinfo.trajectorypoints.size());
//         ROS_INFO("-------------------------------Reference Trajectory------------------------------------");
//         for(const auto& point : ref_trajectory_ptr->trajectoryinfo.trajectorypoints)
//         {
//             ROS_INFO("Point x: %f, y: %f, yaw: %f, v: %f, a: %f, kappa: %f",
//                      point.position.x, point.position.y, point.heading,
//                      point.velocity, point.a, point.curvature);
//         }
//         ROS_INFO("---------------------------------------------------------------------------------------");
        // init_point_ref_trajectory_ptr = ref_trajectory_ptr->trajectoryinfo.trajectorypoints[0];
    }

    void ControllerNode::initLonCon(ros::NodeHandle& n) {
        // PID参数
        n.param<double>("distance_kp", distance_kp, 0.85);
        n.param<double>("distance_ki", distance_ki, 0.02);
        n.param<double>("distance_kd", distance_kd, 0.1);
        n.param<double>("velocity_kp", velocity_kp, 0.85);
        n.param<double>("velocity_ki", velocity_ki, 0.02);
        n.param<double>("velocity_kd", velocity_kd, 0.1);
    }

    void ControllerNode::initLatCon(ros::NodeHandle& n) {
        // LQR参数
        double lqr_q1, lqr_q2, lqr_q3;
        double lqr_u1, lqr_u2;
        n.param<double>("lqr_q1", lqr_q1, 1.0);
        n.param<double>("lqr_q2", lqr_q2, 1.0);
        n.param<double>("lqr_q2", lqr_q2, 1.0);
        n.param<double>("lqr_u1", lqr_u1, 1.0);
        n.param<double>("lqr_u2", lqr_u2, 1.0);
        n.param<double>("eps", eps, 0.001);
        n.param<int>("max_iterations", max_iterations, 1000);

        Q.diagonal() << lqr_q1, lqr_q2, lqr_q3;
        R.diagonal() << lqr_u1, lqr_u2;
    }

    void ControllerNode::initVehicleParameters(ros::NodeHandle& n) {
        n.param<double>("l", vehicle_config.l, 4);
        n.param<double>("steering_ratio", vehicle_config.steer_ratio, 16.8);
        n.param<double>("preview_time", preview_time, 0.01);
//        std::cout << "vehicle_config.l: " << vehicle_config.l << std::endl;
//        std::cout << "preview_time: " << preview_time << std::endl;
    }


} // carla_pnc
